#include "zf_common_headfile.h"

// 测试pid
int motor_test = 0;
int pid_target_speed = 0;

// 四路PWM波接口
#define PWM_CH4   ATOM0_CH0_P21_2   // 左电机倒转
#define PWM_CH3   ATOM0_CH1_P21_3   // 左电机正转
#define PWM_CH2   ATOM0_CH2_P21_4   // 右电机倒转
#define PWM_CH1   ATOM0_CH3_P21_5   // 右电机正转

void motor_init (void)
{
    pwm_init(ATOM0_CH3_P21_5, 17000, 0);
    pwm_init(ATOM0_CH2_P21_4, 17000, 0);
    pwm_init(ATOM0_CH1_P21_3, 17000, 0);
    pwm_init(ATOM0_CH0_P21_2, 17000, 0);
    motor_r.motor_mode = MODE_NORMAL;
    motor_l.motor_mode = MODE_NORMAL;
}

void pwm_control (int32 duty_m1, int32 duty_m2)
{
    //duty_m1右电机 duty_m2左电机

    //限幅
    if (duty_m1 > PWM_DUTY_MAX)
        duty_m1 = PWM_DUTY_MAX;
    else if (duty_m1 < -PWM_DUTY_MAX)
        duty_m1 = -PWM_DUTY_MAX;

    if (duty_m2 > PWM_DUTY_MAX)
        duty_m2 = PWM_DUTY_MAX;
    else if (duty_m2 < -PWM_DUTY_MAX)
        duty_m2 = -PWM_DUTY_MAX;

    // 右电机
    if (duty_m1 >= 0)
    {
        pwm_set_duty(PWM_CH1, duty_m1);
        pwm_set_duty(PWM_CH2, 0);
    }
    else
    {
        pwm_set_duty(PWM_CH1, 0);
        pwm_set_duty(PWM_CH2, -duty_m1);
    }
    // 左电机
    if (duty_m2 >= 0)
    {
        pwm_set_duty(PWM_CH3,duty_m2);
        pwm_set_duty(PWM_CH4,0);
    }
    else
    {
        pwm_set_duty(PWM_CH3, 0);
        pwm_set_duty(PWM_CH4, -duty_m2);
    }
}

void speed_control (void)
{
    int speed = set_speed;
//     if (circle_type != CIRCLE_NONE)
//     {
//          motor_r.target_speed = (int16) (speed+servo_duty)*protect_flag;
//         motor_l.target_speed = (int16) (speed-servo_duty)* protect_flag;
//     }

    if (motor_test != 0)
        motor_r.target_speed = motor_l.target_speed = pid_target_speed;
    else
    {
        motor_r.target_speed = (int16) (speed + servo_duty) * protect_flag;
        motor_l.target_speed = (int16) (speed - servo_duty) * protect_flag;
    }
}

void motor_control (void)
{
    speed_control();

    const int max_pwm = 8000;

    if (motor_l.motor_mode == MODE_NORMAL)
    {
        motor_l.duty += increment_pid_solve(&motor_pid_l, &motor_l);
        motor_l.duty = MINMAX(motor_l.duty, -max_pwm, max_pwm);
    }


//    else if (motor_l.motor_mode == MODE_BANGBANG)
//    {
//        // 閭﹂偊妯″紡锛屽嵆鐚涘姞鐚涘噺
//        motor_pid_l.out_i = 0;  // 娓呴櫎榛樿妯″紡鐨勭Н鍒嗛噺
//
//        motor_l.duty += bangbang_pid_solve(&motor_pid_l_bangbang, &motor_l);
//        motor_l.duty = MINMAX(motor_l.duty, -MOTOR_PWM_DUTY_MAX, MOTOR_PWM_DUTY_MAX);
//    }
//    else if (motor_l.motor_mode == MODE_SOFT)
//    {
//        // 缂撳惎鍔ㄦā寮�
//        motor_pid_l.out_i = 0;  // 娓呴櫎榛樿妯″紡鐨勭Н鍒嗛噺
//
//        motor_l.duty += changable_pid_solve(&motor_pid_l_bangbang, &motor_l);
//        motor_l.duty = MINMAX(motor_l.duty, -MOTOR_PWM_DUTY_MAX, MOTOR_PWM_DUTY_MAX);
//    }

    // 鍙崇數鏈猴紝鍚屽乏鐢垫満
    if (motor_r.motor_mode == MODE_NORMAL)
    {
        motor_r.duty += increment_pid_solve(&motor_pid_r, &motor_r);
        motor_r.duty = MINMAX(motor_r.duty, -max_pwm, max_pwm);
    }

//    motor_r.duty = motor_l.duty = 2000;
    pwm_control(motor_r.duty, motor_l.duty);
}

float real_speed(int speed)
{
    return (float)speed*(0.000084904058/0.005);
}
